#include "iec61850_server.h"
#include "hal_thread.h"
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <arpa/inet.h>
#include <linux/un.h>

#include <libxml/parser.h>
#include <libxml/xmlmemory.h>
#include "ipc_socket.h"
#include "modbus_ipc.h"
#include "static_model.h"
#include "pcs_modbus_registers.h"

extern IedModel iedModel;
static IedServer iedServer = NULL;

static void modbus_TLV_process(void)
{
    char *dataPtr = NULL;
    modbus_common *modbusData = NULL;
    int size = 0;
    int recvLen = 0;
    int i = 0, j = 0, k = 0;
    int addrStart = 0, addrEnd = 0;
    RegisterDesc *recvRegister = NULL;
    RegisterBitMap *bitMap = NULL;
    uint64_t timeStamp;
    uint16_t dataValue = 0;

    size = sizeof(modbus_common);
    dataPtr = (char *)malloc(size);
    memset(dataPtr, 0, size);
    //处理MODBUS的TLV数据
    recvLen = get_TLV_data(MODBUS_IPC, (void *)dataPtr, size);
    while(recvLen) {
        if (recvLen >= sizeof(modbus_common)) {
            timeStamp = Hal_getTimeInMs();
            modbusData = (modbus_common *)dataPtr;
#ifdef DEBUG
            printf("Type = %d Addr = %d Data = %d\n", modbusData->type, modbusData->addr, modbusData->value);
#endif
            i = 0;
            while (modbusRegistersArray[i].addrStart != 0) {
                addrStart = modbusRegistersArray[i].addrStart;
                addrEnd = modbusRegistersArray[i].addrEnd;
                if ((modbusData->addr < addrStart) || (modbusData->addr > addrEnd)) {
                    i ++;
                    continue;
                }
                recvRegister = modbusRegistersArray[i].registerRecvArray;
                j = 0;
                while (recvRegister[j].addr != 0) {
                    if (recvRegister[j].addr != modbusData->addr) {
                        j ++;
                        continue;
                    }

                    if (recvRegister[j].registerBitMap == NULL) {
                        if (recvRegister[j].scale == 1.0) {
                            IedServer_updateInt32AttributeValue(iedServer, recvRegister[j].dataPtr, modbusData->value);
                        } else {
                            IedServer_updateFloatAttributeValue(iedServer, recvRegister[j].dataPtr, (float)(modbusData->value * recvRegister[j].scale));
                        }
                        IedServer_updateUTCTimeAttributeValue(iedServer, recvRegister[j].dataT, timeStamp);
                    } else {
                        bitMap = recvRegister[j].registerBitMap;

                        k = 0;
                        while (bitMap[k].dataPtr != NULL) {
                            dataValue =  (uint16_t)modbusData->value;
                            dataValue = dataValue >> bitMap[k].offset;
                            dataValue = dataValue & (0xFFFF >> (16 - bitMap[k].nr));
                            if (bitMap == switchInputStatus) {
                                IedServer_updateInt32AttributeValue(iedServer, bitMap[k].dataPtr, dataValue);
                            } else {
                                IedServer_updateBooleanAttributeValue(iedServer, bitMap[k].dataPtr, (bool)dataValue);
                            }
                            IedServer_updateUTCTimeAttributeValue(iedServer, bitMap[k].dataT, timeStamp);
                            k ++;
                        }
                    }
                    
                    break;
                }
                i ++;
            }
        }
        /*
        uint64_t currentTime = Hal_getTimeInMs();
        IedServer_updateUTCTimeAttributeValue(iedServer, IEDMODEL_GenericIO_TIM_GAPC1_OpCntRs_t, currentTime);
        IedServer_updateBooleanAttributeValue(iedServer,NULL,true);
        IedServer_updateQuality(iedServer, IEDMODEL_GenericIO_GGIO1_SPCSO2_q, QUALITY_VALIDITY_QUESTIONABLE | QUALITY_DETAIL_OSCILLATORY);
        */
        memset(dataPtr, 0, size);
        recvLen = get_TLV_data(MODBUS_IPC, (void *)dataPtr, size);
    }
    free(dataPtr);
}

static void connectionHandler (IedServer self, ClientConnection connection, bool connected, void* parameter)
{
    if (connected)
        printf("Connection opened\n");
    else
        printf("Connection closed\n");
}

static MmsDataAccessError writeAccessHandler (DataAttribute* dataAttribute, MmsValue* value, ClientConnection connection, void* parameter)
{
    float val = 0;
    RegisterDesc *dataSend = (RegisterDesc *) parameter;
    modbus_common modbusData = {0};
    uint64_t timeStamp;
    if ((dataAttribute != NULL) && (dataAttribute == dataSend->dataPtr)) {
        timeStamp = Hal_getTimeInMs();
        if (dataSend->scale == 1.0) {
            val = (float)MmsValue_toInt32(value);
        } else {
            val = MmsValue_toFloat(value);
        }
        val = val / dataSend->scale;
#ifdef DEBUG
        printf("Addr:%d scale:%f value:%f dataPtr:%ld", dataSend->addr, dataSend->scale, val, (long int)dataSend->dataPtr);
#endif
        modbusData.type = MODBUS_WRITE;
        modbusData.addr = dataSend->addr;
        modbusData.value = val;
        send_TLV_data(MODBUS_IPC, (void *)&modbusData, sizeof(modbus_common));
        IedServer_updateUTCTimeAttributeValue(iedServer, dataSend->dataT, timeStamp);
    }
    return DATA_ACCESS_ERROR_SUCCESS;
}

static void init_writeHandler(void)
{
    int i = 0, j =0;
    RegisterDesc *sendArray = NULL;
#ifdef DEBUG
    printf("Enter: %s: %d\n", __FILE__, __LINE__);
#endif
    
    i = 0;
    while(modbusRegistersArray[i].addrStart != 0) {
        if (modbusRegistersArray[i].registerSendArray == NULL) {
            i ++;
            continue;
        }
#ifdef DEBUG
        printf("Start=%d End=%d Recv=%ld Send=%ld\n", 
                modbusRegistersArray[i].addrStart,
                modbusRegistersArray[i].addrEnd,
                (long int)modbusRegistersArray[i].registerRecvArray,
                (long int)modbusRegistersArray[i].registerSendArray);
#endif
        sendArray = modbusRegistersArray[i].registerSendArray;
        j = 0;
        while(sendArray[j].addr != 0){
            /* Install write callback handler for substitution variables */
            IedServer_handleWriteAccess(iedServer, (DataAttribute *)sendArray[j].dataPtr, writeAccessHandler, &sendArray[j]);
            j ++;
        }
        i ++;
    }

}

int init_iec_server()
{
    printf("Using libIEC61850 version %s\n", LibIEC61850_getVersionString());
    /* Create a new IEC 61850 server instance */
    iedServer = IedServer_create(&iedModel);
    IedServer_setConnectionIndicationHandler(iedServer, (IedConnectionIndicationHandler) connectionHandler, NULL);
    init_writeHandler();
    /* MMS server will be instructed to start listening for client connections. */
    IedServer_start(iedServer, 10002);

    if (!IedServer_isRunning(iedServer)) {
        printf("Starting server failed! Exit.\n");
        IedServer_destroy(iedServer);
        exit(-1);
    }
    return 0;
}

int main()
{
    int fd = -1;
    pthread_t thread_socket;
    fd = client_socket_init();
    if(fd == -1) {
        perror("Can't create socket or connect");
    }
    
    if((fd != -1) && (socket_recv_thread(&thread_socket, fd) == -1)) {
        perror("Create thread failed");
        close(fd);
    }

    if ((fd != -1) && (socketfd_bind(MODBUS_IPC, fd) != fd)) {
        perror("Socket fd bind failed\n");
    }

    //初始化IEC61850 server
    init_iec_server();

    while(is_exit()) {   
        modbus_TLV_process();
        Thread_sleep(10);
    }
    /* stop MMS server - close TCP server socket and all client sockets */
    IedServer_stop(iedServer);
    /* Cleanup - free all resources */
    IedServer_destroy(iedServer);

    socketfd_unbind(MODBUS_IPC);
    close(fd);
    return 0;
}
